#include "filter.h"
#include "math.h"

Butter_Parameter Butter_80HZ_Parameter_Acce,Butter_60HZ_Parameter_Acce,Butter_51HZ_Parameter_Acce,
                 Butter_30HZ_Parameter_Acce,Butter_20HZ_Parameter_Acce,Butter_15HZ_Parameter_Acce,
                 Butter_10HZ_Parameter_Acce,Butter_5HZ_Parameter_Acce,Butter_2HZ_Parameter_Acce;
Butter_BufferData Butter_Buffer_ACC_X,Butter_Buffer_ACC_Y,Butter_Buffer_ACC_Z,Butter_Buffer_GyroX,Butter_Buffer_GyroY,Butter_Buffer_GyroZ,Butter_Buffer_Carangle;
Butter_BufferData Butter_Buffer_speed;
/****************************************
    一阶惯性低通滤波器
***************************************/
void LPF_1_db(float hz,float time,float in,float *out)
{
	*out += ( 1 / ( 1 + 1 / ( hz *6.28f *time ) ) ) *( in - *out );
}
/****************************************
    Butterworth低通滤波器参数初始化
***************************************/

void Butterworth_Parameter_Init(void)
{
  set_cutoff_frequency(200, 80,&Butter_80HZ_Parameter_Acce);
  set_cutoff_frequency(200, 60,&Butter_60HZ_Parameter_Acce);
  set_cutoff_frequency(200, 51,&Butter_51HZ_Parameter_Acce);
  set_cutoff_frequency(200, 30,&Butter_30HZ_Parameter_Acce);
  set_cutoff_frequency(200, 20,&Butter_20HZ_Parameter_Acce);
  set_cutoff_frequency(200, 15,&Butter_15HZ_Parameter_Acce);
  set_cutoff_frequency(200, 10,&Butter_10HZ_Parameter_Acce);
  set_cutoff_frequency(200, 5 ,&Butter_5HZ_Parameter_Acce);
  set_cutoff_frequency(200, 2 ,&Butter_2HZ_Parameter_Acce);//速度读取2ms
}


/*************************************************
函数名:	float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
说明:	加速度计低通滤波器
入口:	float curr_input 当前输入加速度计,滤波器参数，滤波器缓存
出口:	无
备注:	2阶Butterworth低通滤波器
*************************************************/

float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
{
        static int LPF_Cnt=0;
        Buffer->Input_Butter[2]=curr_input;
        if(LPF_Cnt>=100)
        {
	/* Butterworth滤波 */
        Buffer->Output_Butter[2]=
         Parameter->b[0] * Buffer->Input_Butter[2]
        +Parameter->b[1] * Buffer->Input_Butter[1]
	+Parameter->b[2] * Buffer->Input_Butter[0]
        -Parameter->a[1] * Buffer->Output_Butter[1]
        -Parameter->a[2] * Buffer->Output_Butter[0];
        }
        else
        {
          Buffer->Output_Butter[2]=Buffer->Input_Butter[2];
          LPF_Cnt++;
        }
	/* x(n) 序列保存 */
        Buffer->Input_Butter[0]=Buffer->Input_Butter[1];
        Buffer->Input_Butter[1]=Buffer->Input_Butter[2];
	/* y(n) 序列保存 */
        Buffer->Output_Butter[0]=Buffer->Output_Butter[1];
        Buffer->Output_Butter[1]=Buffer->Output_Butter[2];
        return Buffer->Output_Butter[2];
}


/// @brief 设置巴特沃斯滤波器通带截至频率
/// @param sample_frequent 采样频率
/// @param cutoff_frequent 截至频率
/// @param LPF 
void set_cutoff_frequency(float sample_frequent, float cutoff_frequent,Butter_Parameter *LPF)
{
	float fr = sample_frequent / cutoff_frequent;
	float ohm = tanf(M_PI_F / fr);
	float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
	if (cutoff_frequent <= 0.0f) {
		// no filtering
		return;
	}
	LPF->b[0] = ohm * ohm / c;
	LPF->b[1] = 2.0f * LPF->b[0];
	LPF->b[2] = LPF->b[0];
        LPF->a[0]=1.0f;
	LPF->a[1] = 2.0f * (ohm * ohm - 1.0f) / c;
	LPF->a[2] = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}


//卡尔曼滤波部分-----
Kalman_Filter KFP;

/// @brief 卡尔曼滤波器初始化
/// @param KFP 滤波器地址
/// @param P 初始估算协方差 建议0.02
/// @param Q 过程噪声协方差 建议0.001
/// @param R 观测噪声协方差 建议0.543
void Kalman_Filter_Init(Kalman_Filter *KFP ,float P,float Q,float R)
{
    KFP->LastP=P;
    KFP->Q=Q;
    KFP->R=R;
}

 /// @brief 卡尔曼滤波器
 /// @param kfp 卡尔曼滤波器地址
 /// @param input 需要滤波的参数的测量值（即传感器的采集值）
 /// @return 滤波后的参数（最优值）
 float KalmanFilter(Kalman_Filter *kfp,float input)
 {
     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
     kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
     return kfp->out;
 }



//均值滤波部分-------------------------------------------------

 int i;
 /// @brief 均值滤波（次数在filter.h中的MEAN_FILTER_NUM）
 /// @param Filter 均值滤波器地址（存放历史数据）
 /// @param input     输入值
 /// @return         输出值
 float MeanFilter(float *Filter, float input)
 {
     float FilterTemp[MEAN_FILTER_NUM + 1]; // 转移数组
     float output;                          // 输出
     FilterTemp[MEAN_FILTER_NUM] = input;
     for (i = 0; i < MEAN_FILTER_NUM; i++)
     {
         output += Filter[i]; // 累计历史输出
         FilterTemp[i] = Filter[i];
     }
	 for(i=0;i<MEAN_FILTER_NUM;i++)
		{
			Filter[i]=FilterTemp[i+1];//保存新的历史值
		}
			
	output = ((float)output)/MEAN_FILTER_NUM;
    return output;
}

/// @brief 均值滤波优化 ,速度更快
/// @param Filter 均值滤波器地址
/// @param input 
/// @return sum
long MeanFilterBetter1(Mean_Filter *Filter,int input)
{
    unsigned int i;
    long sum=0;
  
  Filter->count++;
  if(Filter->count >= 65534)
    Filter->count=0;

  if(Filter->count%20==0)
  {
    Filter->Filter[Filter->NewNumPtr]=input;//保存新数
    Filter->NewNumPtr++;//指向下一次新数据
    if(Filter->NewNumPtr==MEAN_FILTER_NUM+1)
      Filter->NewNumPtr=0;
  }
   
   for(i = 0;i<MEAN_FILTER_NUM;i++)
   {
        sum+=Filter->Filter[i];
   }
   sum /= MEAN_FILTER_NUM;
    return sum;  
}

//低通滤波部分--------------------

/// @brief 低通滤波
/// @param Filter 滤波器地址
/// @param nData 新数据
/// @param nType 数据类型
void LPF(LPF_Filter *Filter, void *nData, unsigned char nType)//低通滤波器
{
  switch(nType)
  {
    case u8data:
      Filter->u8Data[3] = Filter->u8Data[2];
      Filter->u8Data[2] = Filter->u8Data[1];
      Filter->u8Data[1] = Filter->u8Data[0];
      Filter->u8Data[0] = *(unsigned char *)nData;
      Filter->u8Data[0] = Filter->u8Data[0]*0.15+0.85*(0.4*Filter->u8Data[0]+0.3*Filter->u8Data[1]+0.2*Filter->u8Data[2]+0.1*Filter->u8Data[3]);
      *(unsigned char *)nData = (unsigned char)Filter->u8Data[0];
      break;
    case u16data:
      Filter->u16Data[3] = Filter->u16Data[2];
      Filter->u16Data[2] = Filter->u16Data[1];
      Filter->u16Data[1] = Filter->u16Data[0];
      Filter->u16Data[0] = *(unsigned short *)nData;
      Filter->u16Data[0] = Filter->u16Data[0]*0.15+0.85*(0.4*Filter->u16Data[0]+0.3*Filter->u16Data[1]+0.2*Filter->u16Data[2]+0.1*Filter->u16Data[3]);
      *(unsigned short *)nData = (unsigned short)Filter->u16Data[0];
      break;
      case s32data:
      Filter->s32Data[3] = Filter->s32Data[2];
      Filter->s32Data[2] = Filter->s32Data[1];
      Filter->s32Data[1] = Filter->s32Data[0];
      Filter->s32Data[0] = *(long *)nData;
      Filter->s32Data[0] = Filter->s32Data[0]*0.15+0.85*(0.4*Filter->s32Data[0]+0.3*Filter->s32Data[1]+0.2*Filter->s32Data[2]+0.1*Filter->s32Data[3]);
      *(long *)nData = (long)Filter->s32Data[0];
      break;
      case u32data:
      Filter->u32Data[3] = Filter->u32Data[2];
      Filter->u32Data[2] = Filter->u32Data[1];
      Filter->u32Data[1] = Filter->u32Data[0];
      Filter->u32Data[0] = *(unsigned long *)nData;
      Filter->u32Data[0] = Filter->u32Data[0]*0.15+0.85*(0.4*Filter->u32Data[0]+0.3*Filter->u32Data[1]+0.2*Filter->u32Data[2]+0.1*Filter->u32Data[3]);
      *(unsigned long *)nData = (unsigned long)Filter->u32Data[0];
      break;
      case s16data:
      Filter->s16Data[3] = Filter->s16Data[2];
      Filter->s16Data[2] = Filter->s16Data[1];
      Filter->s16Data[1] = Filter->s16Data[0];
      Filter->s16Data[0] = *(short *)nData;
      Filter->s16Data[0] = Filter->s16Data[0]*0.15+0.85*(0.4*Filter->s16Data[0]+0.3*Filter->s16Data[1]+0.2*Filter->s16Data[2]+0.1*Filter->s16Data[3]);
      *(short *)nData = (short)Filter->s16Data[0];
      break;
    case s8data:
      Filter->s8Data[3] = Filter->s8Data[2];
      Filter->s8Data[2] = Filter->s8Data[1];
      Filter->s8Data[1] = Filter->s8Data[0];
      Filter->s8Data[0] = *(char *)nData;
      Filter->s8Data[0] = Filter->s8Data[0]*0.15+0.85*(0.4*Filter->s8Data[0]+0.3*Filter->s8Data[1]+0.2*Filter->s8Data[2]+0.1*Filter->s8Data[3]);
      *(char *)nData = (char)Filter->s8Data[0];
      break;
      case fdata:
      Filter->fData[3] = Filter->fData[2];
      Filter->fData[2] = Filter->fData[1];
      Filter->fData[1] = Filter->fData[0];
      Filter->fData[0] = *(float *)nData;
      Filter->fData[0] = Filter->fData[0]*0.15+0.85*(0.4*Filter->fData[0]+0.3*Filter->fData[1]+0.2*Filter->fData[2]+0.1*Filter->fData[3]);
      *(float *)nData = (float)Filter->fData[0];
      break;
  }
}



    